//
//  Vehicle.m
//  Racing
//
//  Created by vy phan on 4/12/11.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#import "Vehicle.h"
#import "State.h"

@implementation Vehicle
-(id) initWithId:(char)vId X:(float)x Y:(float)y
{
	self = [self initWithX:x Y:y];
	if (self != nil) {
		vehicleId = vId;
	}
	return self;
}
-(id) initWithX:(float)x Y:(float)y
{
	self = [super init];
	if (self != nil) {
		motoron = NO;
		reversing = NO;
		steering_angle = 0.0f;
		wx1 = 0.0f;
		wy1 = 0.0f;
		wx2 = 0.0f;
		wy2 = 0.0f;
		length = VEHICLE_HEIGHT;
		width = VEHICLE_WIDTH;
		vehicleId = (char)0;
		
		[self currentState]->x = x;
		[self currentState]->y = y;
		[self currentState]->xpoints = [[NSMutableArray alloc]initWithCapacity:4];
		[self currentState]->ypoints = [[NSMutableArray alloc]initWithCapacity:4];
		[self currentState]->numpoints = 4;
		
		[self nextState]->xpoints = [[NSMutableArray alloc]initWithCapacity:4];
		[self nextState]->ypoints = [[NSMutableArray alloc]initWithCapacity:4];
		[self nextState]->numpoints = 4;
		
		for (int i = 0; i < 4; i++) {
			[[self nextState]->xpoints addObject:[NSNumber numberWithFloat:0.0f]];
			[[self nextState]->ypoints addObject:[NSNumber numberWithFloat:0.0f]];
			[[self currentState]->xpoints addObject:[NSNumber numberWithFloat:0.0f]];
			[[self currentState]->ypoints addObject:[NSNumber numberWithFloat:0.0f]];
		}
		
		momentOfInertia = (length * length / 4) * (width * width / 4) / 4;
		mass = 4;
	}
	return self;
}

-(char) getId
{
	return self->vehicleId;
}

-(void) setAngle
{
	if ( left && !right ) {
		steering_angle = (float)(M_PI_4);
	}
	else if ( right && !left ) {
		steering_angle = -(float)(M_PI_4);
	}
	else {
		steering_angle = 0;
	}
}

-(void) steerLeft:(BOOL)left_
{
	self->left = left_;
	[self setAngle];
}

-(void) steerRight:(BOOL)right_
{
	self->right = right_;
	[self setAngle];
}

-(float) getLength
{
	return length;
}

-(float) getWidth
{
	return width;
}


-(void) setMotorOn:(BOOL)on_
{
	motoron = on_;
}

-(void) reverse:(BOOL)reversing_
{
	self->reversing = reversing_;
}

-(void) integrate:(float)dt
{
	State *state = [self currentState];
	
	float angle = state->angle;
	
	float dx = sinf( angle );
	float dy = cosf( angle );
	
	wx1 = 4*dx - 4*dy;
	wy1 = 4*dy + 4*dx;
	wx2 = 4*dx + 4*dy;
	wy2 = 4*dy - 4*dx;
	
	
	if ( motoron || reversing || steering_angle != 0 ) {
		
		if (angle != angle) {
			NSLog(@"NAN");
		}
		
		float forceAngle = angle + steering_angle;
		
		float force = 0;//reversing ? -20 : motoron ? 40 : 20;
		if (reversing) {
			force = -20.0f;
		}
		else {
			if (motoron) {
				force = 40.0f;
			}
			else {
				force = 20.0f;
			}

		}

		float steering_force_x = (force*sinf( forceAngle ));
		float steering_force_y = (force*cosf( forceAngle ));
		if (steering_force_x != steering_force_x || steering_force_y != steering_force_y) {
			NSLog(@"NAN");
		}
		else {
			
			float x = state->x;
			float y = state->y;
			
			[state applyForce:x + wx1 Y:y + wy1 FX:steering_force_x FY:steering_force_y];
			[state applyForce:x + wx2 Y:y + wy2 FX:steering_force_x FY:steering_force_y];
		}

	}
	
	
	[super integrate:dt];
	
	float halfLength = 0.5f * length;
	float halfWidth  = 0.5f * width;
	
	State *next_state = [self nextState];
	printf(">>>>>>>>>>>>>> angle: %f\n", next_state->angle);
	dx = sinf( next_state->angle );
	dy = cosf( next_state->angle );
	
	float x = next_state->x;
	float y = next_state->y;
	
	float x0 = x + dx*halfLength - dy*halfWidth;
	float y0 = y + dy*halfLength + dx*halfWidth;
	for (int j = 0; j < 4; j++) {
		float testx = [[next_state->xpoints objectAtIndex:j] floatValue];
		float testy = [[next_state->ypoints objectAtIndex:j] floatValue];
		printf("next xpoint %d : %f\n", j, testx);
		printf("next ypoint %d : %f\n", j, testy);
	}
	if (x0 != x0 || y1 != y1) {
		NSLog(@"NAN");
	}
	else {
	[next_state->xpoints replaceObjectAtIndex:0 withObject:[NSNumber numberWithFloat:x0]];
	[next_state->ypoints replaceObjectAtIndex:0 withObject:[NSNumber numberWithFloat:y0]];
	
	[next_state->xpoints replaceObjectAtIndex:1 withObject:[NSNumber numberWithFloat:x - dx*halfLength - dy*halfWidth]];
	[next_state->ypoints replaceObjectAtIndex:1 withObject:[NSNumber numberWithFloat:y - dy*halfLength + dx*halfWidth]];
	
	[next_state->xpoints replaceObjectAtIndex:2 withObject:[NSNumber numberWithFloat:x - dx*halfLength + dy*halfWidth]];
	[next_state->ypoints replaceObjectAtIndex:2 withObject:[NSNumber numberWithFloat:y - dy*halfLength - dx*halfWidth]];
	
	[next_state->xpoints replaceObjectAtIndex:3 withObject:[NSNumber numberWithFloat:x + dx*halfLength + dy*halfWidth]];
	[next_state->ypoints replaceObjectAtIndex:3 withObject:[NSNumber numberWithFloat:y + dy*halfLength - dx*halfWidth]];
	
	
	}
}
@end
